#!/usr/bin/env python
import numpy as np
import sys
import os 
import matplotlib.pyplot as plt
import scienceplots
import glob


from range_estimator_4d import get_crlb
from my_common_lib.matplot_common import linestyle, marker, labelfont, titlefont, color
from my_common_lib.common_lib import get_paramter_from_json
# from range_estimator_4d import points_v1, get_traj, get_sparse_indices
from range_estimator_4d import  get_traj_pose as get_traj
from range_estimator_4d import  get_sparse_pose_indices
# from range_estimator_4d import points_v1_for_interpolation as points_v1
from range_estimator_4d import points_v3

points = None 
current_dir = None
sparse_indices = None
fig_style = 1
path_points_len = 2000


def plot_ekf(crlb, rmse, partial_position_rmses, partial_heading_rmse, anchors_list, labels):
    time_stick = np.arange(path_points_len) * 40 / path_points_len
    with plt.style.context(['science','ieee']):
        crlb_np = np.array(crlb).reshape(len(anchors_list),-1,4,4)
        crlb_trace_np = np.trace(crlb_np, axis1=2, axis2=3)
        rmse_np = np.array(rmse).reshape(len(anchors_list),2)
        cm = 1/2.54 # centimeters in inches

        # plot crlb
        # plt.subplot(4, 1, 2)
        plt.figure(1, figsize=(8.5 * cm, 6 * cm*3/4), dpi=600)
        for i in range(len(anchors_list)):
            plt.plot(time_stick, crlb_trace_np[i, :], label= '$' + labels[i] +'$', linestyle="--", color=color[i+4])
        plt.ylabel("Trace of CRLB")
        plt.xlabel("Time (s)")
        
        for idx in sparse_indices:
            # plt.axvline(x=idx, color='gray', linestyle=':', linewidth=1)
            plt.axvline(x=idx*40/path_points_len, color='gray', linestyle=':', linewidth=1)
        # plt.ylim([-0.3, 0.3])
        # plt.legend(ncols = int(len(anchors_list)), fontsize='small')
        # plt.legend(ncols = int(len(anchors_list)))
        plt.legend(ncols = 1, fontsize='small')
        # plt.yscale('log')
        # plt.grid(True)
        plt.savefig(os.path.join(current_dir, "localizaiton_result_4_anchor_trace_crlb.png"), dpi=600)
        plt.close()

        # plot seperate the sigma_t and sigam_gamma
        plt.figure(2, figsize=(8.5 * cm, 6 * cm*3/4), dpi=600)
        # plt.figure(2, figsize=(8.5 * cm, 6 * cm*3/4), dpi=600)
        crlb_trace_t = crlb_np[:,:,0,0] + crlb_np[:,:,1, 1] + crlb_np[:,:,2, 2]
        crlb_trace_gamma = crlb_np[:, :, 3, 3]
        for i in range(len(anchors_list)):
            str1 = '$\sigma_{\mathbf{p},' + labels[i] + '}$'
            str2 = '$\sigma_{\gamma,' + labels[i] + '}$'
            plt.plot(time_stick, crlb_trace_t[i,:], label=str1, linestyle="--", color=color[i+4])
            plt.plot(time_stick, crlb_trace_gamma[i,:], label=str2, linestyle="-", color=color[i+4])
        plt.ylabel("Value") 
        plt.xlabel("Time (s)") 
        # sparse_indices = get_sparse_pose_indices(points, number=5300)
        for idx in sparse_indices:
            plt.axvline(x=idx * 40 / path_points_len, color='gray', linestyle=':', linewidth=1)
        # plt.ylim([-0.3, 0.3])
        # plt.legend(ncols = int(len(anchors_list)), fontsize='small')
        plt.legend(ncols = 2,  bbox_to_anchor=(0.55, 0.35), fontsize='small')
        # plt.legend(ncols = int(len(anchors_list)))
        # plt.yscale('log')
        # plt.grid(True)
        plt.savefig(os.path.join(current_dir, "localizaiton_result_4_anchor_trace_crlb_seperately.png"), dpi=600)
        plt.close()

        # plot rmse
        # fig, ax1 = plt.subplots(figsize=(8.5*cm*2,6*cm*3/4), dpi=600)
        fig, ax1 = plt.subplots(figsize=(8.5*cm,6*cm*3/4), dpi=600)
        ax2 = ax1.twinx()
        line_pos = ax1.plot(rmse_np[:,0], color='r', marker = marker[0], label='$\mathbf{p}$')
        # line_pos2 = ax1.plot(partial_position_rmses, color='r', marker = marker[3], label='$\mathbf{p}$')
        line_gamma = ax2.plot(rmse_np[:,1], color='b', marker = marker[1], label='$\gamma$')
        # line_gamma2 = ax2.plot(partial_heading_rmse, color='b', marker = marker[2], label='$\gamma_p$')
        ax1.set_ylabel('RMSE of Position (m)', family='Times New Roman')
        ax1.set_xlabel('Anchor Number', family='Times New Roman')
        ax2.set_ylabel('RMSE of Yaw (rad)', family='Times New Roman')
        # step = 1  # 每隔 2 个显示一个标签
        # ax1.set_xticks(np.arange(0, len(anchors_list), step))
        # ax1.set_xticklabels(labels[::step], rotation=0, ha='right', family='Times New Roman')
        list_labels = ['${}$'.format(labels[i]) for i in range(len(labels))]
        plt.xticks(np.arange(len(anchors_list)), list_labels,family='Times New Roman')
        handles1, labels1 = ax1.get_legend_handles_labels()
        handles2, labels2 = ax2.get_legend_handles_labels()
        # ax1.legend(handles1, labels1, loc='upper right', bbox_to_anchor=(0.6, 1), fontsize='small')
        # ax2.legend(handles2, labels2, loc='upper right', bbox_to_anchor=(0.9, 1), fontsize='small')
        ax1.legend(handles1, labels1, loc='upper right', bbox_to_anchor=(0.75, 0.6), fontsize='small')
        ax2.legend(handles2, labels2, loc='upper right', bbox_to_anchor=(0.75, 0.4), fontsize='small')
        # ax1.set_ylim(0.05, 0.22)
        # ax2.set_ylim(0.03, 0.07)
        plt.tight_layout()
        plt.savefig(os.path.join(current_dir, "localizaiton_result_4_anchor_rmse.png"), dpi=600)
        plt.close()
        plt.show()
        print(labels)
        print('rmse position: ', rmse_np[:,0])
        print('heading position: ', rmse_np[:,1])
        # print('rmse position: ', partial_heading_rmse)


def plot_ekf_style_1(crlb, rmse, partial_position_rmses, partial_heading_rmse, anchors_list, labels):
    time_stick = np.arange(path_points_len) * 40 / path_points_len
    with plt.style.context(['science','ieee']):
        crlb_np = np.array(crlb).reshape(len(anchors_list),-1,4,4)
        crlb_trace_np = np.trace(crlb_np, axis1=2, axis2=3)
        rmse_np = np.array(rmse).reshape(len(anchors_list),2)
        cm = 1/2.54 # centimeters in inches

        # plot crlb
        # plt.subplot(4, 1, 2)
        plt.figure(1, figsize=(8.5 * cm, 6 * cm*3/4), dpi=600)
        for i in range(len(anchors_list)):
            plt.plot(time_stick, crlb_trace_np[i, :], label= '$' + labels[i] +'$', linestyle="--", color=color[i+4])
        plt.ylabel("Trace of CRLB")
        plt.xlabel("Time (s)") 
        
        for idx in sparse_indices:
            # plt.axvline(x=idx, color='gray', linestyle=':', linewidth=1)
            plt.axvline(x=idx* 40 / path_points_len, color='gray', linestyle=':', linewidth=1)
        # plt.ylim([-0.3, 0.3])
        # plt.legend(ncols = int(len(anchors_list)), fontsize='small')
        # plt.legend(ncols = int(len(anchors_list)))
        plt.legend(ncols = 1, fontsize='small', bbox_to_anchor=(0.65, 0.25))
        plt.yscale('log')
        # plt.grid(True)
        plt.savefig(os.path.join(current_dir, "localizaiton_result_4_anchor_trace_crlb.png"), dpi=600)
        plt.close()

        # plot seperate the sigma_t and sigam_gamma
        plt.figure(2, figsize=(8.5 * cm, 6 * cm), dpi=600)
        # plt.figure(2, figsize=(8.5 * cm, 6 * cm*3/4), dpi=600)
        crlb_trace_t = crlb_np[:,:,0,0] + crlb_np[:,:,1, 1] + crlb_np[:,:,2, 2]
        crlb_trace_gamma = crlb_np[:, :, 3, 3]
        for i in range(len(anchors_list)):
            str1 = '$\sigma_{\mathbf{p},' + labels[i] + '}$'
            str2 = '$\sigma_{\gamma,' + labels[i] + '}$'
            plt.plot(time_stick, crlb_trace_t[i,:], label=str1, linestyle="--", color=color[i+4])
            plt.plot(time_stick, crlb_trace_gamma[i,:], label=str2, linestyle="-", color=color[i+4])
        plt.ylabel("Value") 
        plt.xlabel("Time (s)") 
        # sparse_indices = get_sparse_pose_indices(points, number=5300)
        for idx in sparse_indices:
            plt.axvline(x=idx * 40 / path_points_len, color='gray', linestyle=':', linewidth=1)
        # plt.ylim([-0.3, 0.3])
        plt.legend(ncols = int(len(anchors_list)), fontsize=5, bbox_to_anchor=(1.0, -0.3))
        # plt.legend(ncols = 2,  bbox_to_anchor=(0.55, 0.35), fontsize='small')
        # plt.legend(ncols = int(len(anchors_list)))
        plt.yscale('log')
        # plt.grid(True)
        plt.tight_layout()
        plt.savefig(os.path.join(current_dir, "localizaiton_result_4_anchor_trace_crlb_seperately.png"), dpi=600)
        plt.close()

        # plot rmse
        # fig, ax1 = plt.subplots(figsize=(8.5*cm*2,6*cm*3/4), dpi=600)
        fig, ax1 = plt.subplots(figsize=(8.5*cm,6*cm*3/4), dpi=600)
        ax2 = ax1.twinx()
        line_pos = ax1.plot(rmse_np[:,0], color='r', marker = marker[0], label='$\mathbf{p}$')
        # line_pos2 = ax1.plot(partial_position_rmses, color='r', marker = marker[3], label='$\mathbf{p}$')
        line_gamma = ax2.plot(rmse_np[:,1], color='b', marker = marker[1], label='$\gamma$')
        # line_gamma2 = ax2.plot(partial_heading_rmse, color='b', marker = marker[2], label='$\gamma_p$')
        ax1.set_ylabel('RMSE of Position (m)', family='Times New Roman')
        ax1.set_xlabel('Anchor Number', family='Times New Roman')
        ax2.set_ylabel('RMSE of Yaw (rad)', family='Times New Roman')
        # step = 1  # 每隔 2 个显示一个标签
        # ax1.set_xticks(np.arange(0, len(anchors_list), step))
        # ax1.set_xticklabels(labels[::step], rotation=0, ha='right', family='Times New Roman')
        list_labels = ['${}$'.format(labels[i]) for i in range(len(labels))]
        plt.xticks(np.arange(len(anchors_list)), list_labels,family='Times New Roman')
        handles1, labels1 = ax1.get_legend_handles_labels()
        handles2, labels2 = ax2.get_legend_handles_labels()
        # ax1.legend(handles1, labels1, loc='upper right', bbox_to_anchor=(0.6, 1), fontsize='small')
        # ax2.legend(handles2, labels2, loc='upper right', bbox_to_anchor=(0.9, 1), fontsize='small')
        ax1.legend(handles1, labels1, loc='upper right', bbox_to_anchor=(0.75, 0.6), fontsize='small')
        ax2.legend(handles2, labels2, loc='upper right', bbox_to_anchor=(0.75, 0.4), fontsize='small')
        # ax1.set_ylim(0.05, 0.22)
        # ax2.set_ylim(0.03, 0.07)
        plt.tight_layout()
        plt.savefig(os.path.join(current_dir, "localizaiton_result_4_anchor_rmse.png"), dpi=600)
        plt.close()
        plt.show()
        print(labels)
        print('rmse position: ', rmse_np[:,0])
        print('heading position: ', rmse_np[:,1])
        # print('rmse position: ', partial_heading_rmse)

def get_rmse(result_path, anchors_num):

    pattern = os.path.join(result_path, "localization_result_*_anchors_*.txt")
    est_matching_files = glob.glob(pattern)
    if est_matching_files:
        print(f"Found {len(est_matching_files)} files:")
    matching_files = [f for f in est_matching_files if "_anchors_gt" not in os.path.basename(f)]

    pattern = os.path.join(result_path, "localization_result_*_anchors_gt.txt")
    gt_matching_files = glob.glob(pattern)
    if gt_matching_files:
        print(f"Found {len(gt_matching_files)} gt files:")
    else:
        print("No matching gt files found.")
        raise FileNotFoundError
    groundtruth_path = gt_matching_files[0]

    # groundtruth_path = os.path.join(result_path, "localization_result_{}_anchors_gt.txt".format(anchors_num))
    groundtruth = np.loadtxt(groundtruth_path)
    true_positions = groundtruth[:, :3]
    true_heading =  groundtruth[:, 5]

    position_rmse_list = []
    heading_rmse_list = []
    partial_heading_rmse_list = []
    partial_position_rmse_list = []
    for i in range(len(matching_files)):
        estimation_path = matching_files[i]
        estimation = np.loadtxt(estimation_path)
        # handle the data, calculate the RMSE of translation and rotation
        estimated_positions = estimation[:, :3]
        estimated_heading = estimation[:, 3]

        position_errors = np.linalg.norm(estimated_positions - true_positions, axis=1) # 计算位置RMSE (欧几里得距离)
        position_rmse = np.sqrt(np.mean(position_errors**2))
        heading_errors = estimated_heading - true_heading # 计算航向RMSE
        heading_rmse = np.sqrt(np.mean(heading_errors**2)) 
        # caluculate RMSE of partial trajectory
        # partial_position_errors = np.linalg.norm(estimated_positions[sparse_indices[2]:sparse_indices[4]] - true_positions[sparse_indices[2]:sparse_indices[4]], axis=1)
        # partial_position_rmse = np.sqrt(np.mean(partial_position_errors**2))
        # partial_heading_rmse =  np.sqrt(np.mean((estimated_heading[sparse_indices[2]:sparse_indices[4]] - true_heading[sparse_indices[2]:sparse_indices[4]])**2)) 
        partial_position_rmse = 0
        partial_heading_rmse = 0

        position_rmse_list.append(position_rmse)
        heading_rmse_list.append(heading_rmse)
        partial_heading_rmse_list.append(partial_heading_rmse)
        partial_position_rmse_list.append(partial_position_rmse)
    position_rmse, heading_rmse, partial_position_rmse, partial_heading_rmse = np.mean(position_rmse_list), np.mean(heading_rmse_list), np.mean(partial_position_rmse_list), np.mean(partial_heading_rmse_list)
    return position_rmse, heading_rmse, partial_position_rmse, partial_heading_rmse

def get_crlb_of_path(path, anchor_num, obj_type):
    pattern = os.path.join(path, "*_optimal_with_*_anchors.txt")
    matching_files = glob.glob(pattern)
    if matching_files:
        print(f"Found {len(matching_files)} anchor config files:")
    else:
        print("No matching files found.")
        raise FileNotFoundError
    anchors_config_path = matching_files[0]
    # anchors_config_path = os.path.join(path, "{}_optimal_with_{}_anchors.txt".format(obj_type, anchor_num))
    anchors = np.loadtxt(anchors_config_path)
    tags = np.array([[0, 0.2, -0.27],
                        [0, -0.2, -0.27]])

    path_points = get_traj(sparse_traj=points, number=path_points_len) #（3.3+4.0+3.3）/0.1*50
    crlb_values = []
    for path_point in path_points:
        crlb = get_crlb(anchors, tags, path_point)
        crlb_values.append(crlb)
    return crlb_values
def handle_result(result_path, obj_type, anchors_num, labels, plot_stype = 0):
    global sparse_indices
    sparse_indices = get_sparse_pose_indices(points, number=path_points_len) # 4 / 0.1 * 50
    rmse = []
    crlb = []
    partial_position_rmses = []
    partial_heading_rmses = []
    
    for i in range(len(result_path)):
        position_rmse, heading_rmse, partial_position_rmse, partial_heading_rmse = get_rmse(os.path.abspath(result_path[i]), anchors_num[i])
        rmse.append([position_rmse, heading_rmse])
        partial_heading_rmses.append(partial_heading_rmse)
        partial_position_rmses.append(partial_position_rmse)
        crlb.append(get_crlb_of_path(os.path.abspath(result_path[i]), anchors_num[i], obj_type[i]))
    if plot_stype == 0:
        plot_ekf(crlb, rmse, partial_position_rmses, partial_heading_rmses,  result_path, labels)
    elif plot_stype == 1:
        plot_ekf_style_1(crlb, rmse, partial_position_rmses, partial_heading_rmses,  result_path, labels)


def comparison_4_result():
    
    obj_type = ['mixed', 'pose', 'pose', 'pose','position']
    labels = [r'\text{mixed Optimal}', '$\\text{D}_1$', '$\\text{D}_2$', '$\\text{D}_3$', r'$\text{Pisition Optimal}$']
    anchors_num = [4, 41, 42, 43, 4]
    result_path = [
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\29',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\41_comparison',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\42_comparison',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\43_comparison',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\27'
    ]
    global points, current_dir
    points = points_v3
    current_dir = os.path.abspath(r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\comparison_4_result_2')
    handle_result(result_path, obj_type, anchors_num, labels)

def comparison_4_result_2():
    obj_type = ['mixed','position', 'orientation', 'pose', 'pose', 'pose']
    labels = [r'\text{Mixed}', r'$\text{Position}$', r'$\text{Orientation}$', '$\\text{D}_1$', '$\\text{D}_2$', '$\\text{D}_3$']
    anchors_num = [4, 4, 4, 41, 42, 43]
    result_path = [
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\29',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\27',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\28',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\41_comparison',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\42_comparison',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\43_comparison'
    ]
    global points, current_dir
    points = points_v3
    current_dir = os.path.abspath(r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\comparison_4_result_3')
    handle_result(result_path, obj_type, anchors_num, labels)

def comparison_4_result_4():
    obj_type = ['mixed2','position', 'orientation', 'pose', 'pose', 'pose']
    labels = [r'\text{Mixed2}', r'$\text{Position}$', r'$\text{Orientation}$', '$\\text{D}_1$', '$\\text{D}_2$', '$\\text{D}_3$']
    anchors_num = [4, 4, 4, 41, 42, 43]
    result_path = [
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\30',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\27',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\28',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\41_comparison',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\42_comparison',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\43_comparison'
    ]
    global points, current_dir
    points = points_v3
    current_dir = os.path.abspath(r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\comparison_4_result_4')
    handle_result(result_path, obj_type, anchors_num, labels)

def comparison_4_result_5():
    obj_type = ['mixed3','position', 'orientation', 'pose', 'pose', 'pose']
    labels = [r'\text{Mixed3}', r'$\text{Position}$', r'$\text{Orientation}$', '$\\text{D}_1$', '$\\text{D}_2$', '$\\text{D}_3$']
    anchors_num = [4, 4, 4, 41, 42, 43]
    result_path = [
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\31',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\27',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\28',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\41_comparison',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\42_comparison',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\43_comparison'
    ]
    global points, current_dir
    points = points_v3
    current_dir = os.path.abspath(r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\comparison_4_result_5')
    handle_result(result_path, obj_type, anchors_num, labels)

def comparison_4_result_6():
    obj_type = ['mixed', 'mixed2', 'mixed3', 'mixed4', 'position', 'pose', 'pose', 'pose']
    labels = [r'\text{Mixed}', r'\text{Mixed2}', r'\text{Mixed3}', r'\text{Mixed4}', r'$\text{Position}$', '$\\text{D}_1$', '$\\text{D}_2$', '$\\text{D}_3$']
    anchors_num = [4, 4, 4, 4, 4, 41, 42, 43]
    result_path = [
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\34',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\35',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\36',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\37',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\27',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\41_comparison',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\42_comparison',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\43_comparison'
    ]
    global points, current_dir
    points = points_v3
    current_dir = os.path.abspath(r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\comparison_4_result_6')
    handle_result(result_path, obj_type, anchors_num, labels)

def comparison_4_result_7():
    obj_type = ['mixed6', 'position', 'pose', 'pose', 'pose']
    labels = [r'\text{Mixed6}', r'$\text{Position}$', '$\\text{D}_1$', '$\\text{D}_2$', '$\\text{D}_3$']
    anchors_num = [4, 4, 41, 42, 43]
    result_path = [
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\39',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\27',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\41_comparison',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\42_comparison',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\43_comparison'
    ]
    global points, current_dir
    points = points_v3
    current_dir = os.path.abspath(r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\comparison_4_result_7')
    handle_result(result_path, obj_type, anchors_num, labels)

def comparison_4_result_8():
    obj_type = ['mixed', 'mixed2', 'mixed3', 'mixed4', 'mixed5', 'mixed6', 'position', 'pose', 'pose', 'pose']
    labels = [r'\text{Mixed}', r'\text{Mixed2}', r'\text{Mixed3}', r'\text{Mixed4}', r'\text{Mixed5}',  r'\text{Mixed6}', r'$\text{Position}$', '$\\text{D}_1$', '$\\text{D}_2$', '$\\text{D}_3$']
    anchors_num = [4, 4, 4, 4, 4,  4, 4, 41, 42, 43]
    result_path = [
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\34',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\35',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\36',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\37',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\38',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\39',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\27',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\41_comparison',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\42_comparison',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\43_comparison'
    ]
    global points, current_dir
    points = points_v3
    current_dir = os.path.abspath(r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\comparison_4_result_8')
    handle_result(result_path, obj_type, anchors_num, labels)

def comparison_4_result_9():
    dynamic_obj_folder_id = range(40, 56)
    dynamic_obj = ['dynamic_mixed'] * len(dynamic_obj_folder_id)
    dynamic_anchors_num = [4] * len(dynamic_obj_folder_id)
    dynamic_labels = []
    dynamic_result_path = []
    dynamic_factor = []
    for i in range(len(dynamic_obj_folder_id)):
        dynamic_result_path.append(os.path.join(r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma', str(dynamic_obj_folder_id[i])))
        factor = get_paramter_from_json(os.path.join(dynamic_result_path[i], 'params.json'), 'dynamic_factor')
        dynamic_factor.append(factor)
        dynamic_labels.append(r'$\text{f}_{' + str(factor) + '}$')
    # sort
    dynamic_result_path, dynamic_obj, dynamic_anchors_num, dynamic_labels, dynamic_factor = (list(t) for t in zip(*sorted(zip(dynamic_result_path, dynamic_obj, dynamic_anchors_num, dynamic_labels, dynamic_factor), key=lambda x: x[4])))


    obj_type = ['pose', 'pose', 'pose', 'pose']
    labels = ['$\\text{Pose}$', '$\\text{D}_1$', '$\\text{D}_2$', '$\\text{D}_3$']
    anchors_num = [4, 41, 42, 43]
    result_path = [
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\56',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\41_comparison',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\42_comparison',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\43_comparison'
    ]
    result_path.extend(dynamic_result_path)
    obj_type.extend(dynamic_obj)
    anchors_num.extend(dynamic_anchors_num)
    labels.extend(dynamic_labels)

    global points, current_dir
    points = points_v3
    current_dir = os.path.abspath(r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\comparison_4_result_9')
    handle_result(result_path, obj_type, anchors_num, labels)

def comparison_4_result_10():
    obj_type = ['pose2', 'pose', 'pose', 'pose', 'pose']
    labels = [r'\text{Optimal}', r'$\text{Pose}$', '$\\text{D}_1$', '$\\text{D}_2$', '$\\text{D}_3$']
    anchors_num = [4, 4, 41, 42, 43]
    result_path = [
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\57',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\56',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\41_comparison',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\42_comparison',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\43_comparison'
    ]
    global points, current_dir
    points = points_v3[2:5,:]
    current_dir = os.path.abspath(r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\comparison_4_result_10')
    handle_result(result_path, obj_type, anchors_num, labels)

def comparison_4_result_11():
    obj_type = ['Optiam', 'position', 'pose', 'pose', 'pose', 'pose']
    labels = [r'\text{Optimal}', r'$\text{Position}$', r'$\text{Pose}$', '$\\text{D}_1$', '$\\text{D}_2$', '$\\text{D}_3$']
    anchors_num = [4, 4, 4, 41, 42, 43]
    result_path = [
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\57',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\27',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\56',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\41_comparison',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\42_comparison',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\43_comparison'
    ]
    global points, current_dir
    points = points_v3[2:5,:]
    current_dir = os.path.abspath(r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\comparison_4_result_11')
    handle_result(result_path, obj_type, anchors_num, labels)

# 仅考虑水平点
def comparison_4_result_12():
    dynamic_obj_folder_id = [33, 58, 59, 60, 61, 62]
    dynamic_obj = ['dynamic_mixed2'] * len(dynamic_obj_folder_id)
    dynamic_anchors_num = [4] * len(dynamic_obj_folder_id)
    dynamic_labels = []
    dynamic_result_path = []
    dynamic_factor = []
    for i in range(len(dynamic_obj_folder_id)):
        dynamic_result_path.append(os.path.join(r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma', str(dynamic_obj_folder_id[i])))
        factor = get_paramter_from_json(os.path.join(dynamic_result_path[i], 'params.json'), 'dynamic_factor')
        dynamic_factor.append(factor)
        dynamic_labels.append(r'$\text{f}_{' + str(factor) + '}$')
    # sort
    dynamic_result_path, dynamic_obj, dynamic_anchors_num, dynamic_labels, dynamic_factor = (list(t) for t in zip(*sorted(zip(dynamic_result_path, dynamic_obj, dynamic_anchors_num, dynamic_labels, dynamic_factor), key=lambda x: x[4])))


    obj_type = ['pose', 'pose', 'pose', 'pose']
    labels = ['$\\text{Pose}$', '$\\text{D}_1$', '$\\text{D}_2$', '$\\text{D}_3$']
    anchors_num = [4, 41, 42, 43]
    result_path = [
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\56',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\41_comparison',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\42_comparison',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\43_comparison'
    ]
    result_path.extend(dynamic_result_path)
    obj_type.extend(dynamic_obj)
    anchors_num.extend(dynamic_anchors_num)
    labels.extend(dynamic_labels)

    global points, current_dir
    points = points_v3
    current_dir = os.path.abspath(r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\comparison_4_result_12')
    handle_result(result_path, obj_type, anchors_num, labels)


# 不同参数的进考虑水平点，包括了result 12,
def comparison_4_result_13():
    dynamic_obj_folder_id = [33, 58, 59, 60, 61, 62, 63, 64, 65, 66, 67, 68, 69, 70, 71, 72]
    dynamic_obj = ['dynamic_mixed2'] * len(dynamic_obj_folder_id)
    dynamic_anchors_num = [4] * len(dynamic_obj_folder_id)
    dynamic_labels = []
    dynamic_result_path = []
    dynamic_factor = []
    for i in range(len(dynamic_obj_folder_id)):
        dynamic_result_path.append(os.path.join(r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma', str(dynamic_obj_folder_id[i])))
        factor = get_paramter_from_json(os.path.join(dynamic_result_path[i], 'params.json'), 'dynamic_factor')
        dynamic_factor.append(factor)
        dynamic_labels.append(r'$\text{f}_{' + str(factor) + '}$')
    # sort
    dynamic_result_path, dynamic_obj, dynamic_anchors_num, dynamic_labels, dynamic_factor = (list(t) for t in zip(*sorted(zip(dynamic_result_path, dynamic_obj, dynamic_anchors_num, dynamic_labels, dynamic_factor), key=lambda x: x[4])))
    print(dynamic_result_path, dynamic_labels)


    obj_type = ['pose', 'pose', 'pose', 'pose']
    labels = ['$\\text{Pose}$', '$\\text{D}_1$', '$\\text{D}_2$', '$\\text{D}_3$']
    anchors_num = [4, 41, 42, 43]
    result_path = [
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\56',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\41_comparison',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\42_comparison',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\43_comparison'
    ]
    result_path.extend(dynamic_result_path)
    obj_type.extend(dynamic_obj)
    anchors_num.extend(dynamic_anchors_num)
    labels.extend(dynamic_labels)

    global points, current_dir
    points = points_v3[2:5,:]
    current_dir = os.path.abspath(r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\comparison_4_result_13')
    handle_result(result_path, obj_type, anchors_num, labels)

def comparison_4_result_14():
    dynamic_obj_folder_id = [73, 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, 90]
    dynamic_obj = ['dynamic_mixed2'] * len(dynamic_obj_folder_id)
    dynamic_anchors_num = [4] * len(dynamic_obj_folder_id)
    dynamic_labels = []
    dynamic_result_path = []
    dynamic_factor = []
    for i in range(len(dynamic_obj_folder_id)):
        dynamic_result_path.append(os.path.join(r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma', str(dynamic_obj_folder_id[i])))
        factor = get_paramter_from_json(os.path.join(dynamic_result_path[i], 'params.json'), 'dynamic_factor')
        dynamic_factor.append(factor)
        dynamic_labels.append(r'$\text{f}_{' + str(factor) + '}$')
    # sort
    dynamic_result_path, dynamic_obj, dynamic_anchors_num, dynamic_labels, dynamic_factor = (list(t) for t in zip(*sorted(zip(dynamic_result_path, dynamic_obj, dynamic_anchors_num, dynamic_labels, dynamic_factor), key=lambda x: x[4])))
    print(dynamic_result_path, dynamic_labels)


    obj_type = [ 'pose', 'pose', 'pose']
    labels = [ '$\\text{D}_1$', '$\\text{D}_2$', '$\\text{D}_3$']
    anchors_num = [41, 42, 43]
    result_path = [
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\41_comparison_fullpath',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\42_comparison_fullpath',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\43_comparison_fullpath'
    ]
    result_path.extend(dynamic_result_path)
    obj_type.extend(dynamic_obj)
    anchors_num.extend(dynamic_anchors_num)
    labels.extend(dynamic_labels)

    global points, current_dir
    points = points_v3
    current_dir = os.path.abspath(r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\comparison_4_result_14')
    handle_result(result_path, obj_type, anchors_num, labels)


def comparison_4_result_fianls():

    obj_type = ['dynamic_mixed2', 'pose', 'pose', 'pose']
    labels = ['\\text{Optimal}', '\\text{D}_1', '\\text{D}_2', '\\text{D}_3']
    anchors_num = [4, 41, 42, 43]
    result_path = [
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\80',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\41_comparison_fullpath',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\42_comparison_fullpath',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\43_comparison_fullpath'
    ]
    global points, current_dir
    points = points_v3
    current_dir = os.path.abspath(r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\comparison_4_result_finals')
    handle_result(result_path, obj_type, anchors_num, labels)

def comparison_4_result_fianls2():

    obj_type = ['dynamic_mixed2', 'pose', 'pose', 'pose']
    labels = ['\\text{Optimal}', '\\text{D}_1', '\\text{D}_2', '\\text{D}_3']
    anchors_num = [4, 41, 42, 43]
    result_path = [
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\64',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\41_comparison',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\42_comparison',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\43_comparison'
    ]
    global points, current_dir
    points = points_v3[2:5]
    current_dir = os.path.abspath(r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\comparison_4_result_finals2')
    handle_result(result_path, obj_type, anchors_num, labels)

def comparison_different_size_finals():

    obj_type = ['dynamic_mixed2', 'dynamic_mixed2', 'dynamic_mixed2', 'dynamic_mixed2', 'dynamic_mixed2']
    labels = ['\\text{m=2}', '\\text{m=3}', '\\text{m=4}', '\\text{m=5}', '\\text{m=6}']
    anchors_num = [2, 3, 4, 5, 6]
    result_path = [
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\6_2_comparison',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\6_3_comparison',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\6_4_comparison',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\6_5_comparison',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\98',
    ]
    global points, current_dir
    points = points_v3[2:5]
    current_dir = os.path.abspath(r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\comparison_different_size_finals')
    handle_result(result_path, obj_type, anchors_num, labels, 1)

def comparison_different_size_finals2():

    obj_type = ['dynamic_mixed2', 'dynamic_mixed2', 'dynamic_mixed2', 'dynamic_mixed2', 'dynamic_mixed2']
    labels = ['\\text{m=2}', '\\text{m=3}', '\\text{m=4}', '\\text{m=5}', '\\text{m=6}']
    anchors_num = [2, 3, 4, 5, 6]
    result_path = [
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\95',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\96',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\64',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\97',
        r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\98',
    ]
    global points, current_dir
    points = points_v3[2:5]
    current_dir = os.path.abspath(r'C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\comparison_different_size_finals2')
    handle_result(result_path, obj_type, anchors_num, labels, 1)

if __name__ == '__main__':
    
    # comparison_4_result()
    # comparison_4_result_2()
    # comparison_4_result_4()
    # comparison_4_result_5()
    # comparison_4_result_6()
    # comparison_4_result_7()
    # comparison_4_result_9()
    # comparison_4_result_13()
    # comparison_4_result_14()
    # comparison_4_result_fianls()
    # comparison_4_result_fianls2()
    # comparison_different_size_finals2()
    comparison_different_size_finals()
    
